#!/usr/bin/env python

import rospy
from lab_automation.srv import SimoService, SimoServiceResponse
from lab_automation.simo_modbus import SimoModbus


class SimoServiceNode:
    def __init__(self):
        # 初始化 ROS 节点
        rospy.init_node('simo_service_node', anonymous=True)

        # 获取串口参数
        self.port = rospy.get_param('~port', '/dev/simo_device')

        # 初始化 Modbus 客户端
        self.simo_modbus = SimoModbus(self.port)

        # 创建服务
        self.service = rospy.Service('simo_service', SimoService, self.handle_request)
        rospy.loginfo("Simo Service 已启动")

    def handle_request(self, req):
        """处理服务请求"""
        operation = req.operation.lower()
        response_message = ""

        try:
            if operation == "start_tear":
                self.simo_modbus.start_tear()
                response_message = "撕膜操作已完成"
            elif operation == "manual_reset":
                self.simo_modbus.manual_reset()
                response_message = "手动复位操作已完成"
            elif operation == "clear_count":
                self.simo_modbus.clear_count()
                response_message = "计数清除操作已完成"
            else:
                response_message = f"未知操作: {req.operation}"
        except Exception as e:
            response_message = f"操作失败: {str(e)}"

        return SimoServiceResponse(response_message)


if __name__ == "__main__":
    try:
        node = SimoServiceNode()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
